// Matrix.cpp: implementation of the CMatrix class.
// Part of KenTools library.
// Written by Kwan-Yee Kenneth Wong 2000.
//
//////////////////////////////////////////////////////////////////////

#include "Matrix.h"
#include <string.h>
#include <math.h>
#include <float.h>
#include <algorithm>
#include <assert.h>

#define singular_value_cutoff (1e-20)

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
template <class Type> 
CMatrix<Type>::CMatrix()
{
	m_nRows = 0;
	m_nCols = 0;
	m_pData = NULL;
}

template <class Type> 
CMatrix<Type>::CMatrix(int r, int c)
{
    m_nRows = 0;
    m_nCols = 0;
    m_pData = NULL;

	SetSize(r, c);
}

template <class Type> 
CMatrix<Type>::CMatrix(int r, int c, Type v)
{
    m_nRows = 0;
    m_nCols = 0;
    m_pData = NULL;

	SetSize(r, c, v);
}

template <class Type>
CMatrix<Type>::CMatrix(const std::vector<Type>& V)
{
    m_nRows = 0;
    m_nCols = 0;
    m_pData = NULL;

    if (!V.empty() && SetSize(V.size(),1))
        memcpy(m_pData, &V[0], sizeof(Type)*m_nRows*m_nCols);
}

template <class Type> 
CMatrix<Type>::CMatrix(const CMatrix<Type>& M)
{
    m_nRows = 0;
    m_nCols = 0;
    m_pData = NULL;

	if (SetSize(M.Rows(), M.Cols()) && M.isValid()) // the isValid call makes sure we only try to memcpy if M isn't empty
		memcpy(m_pData, M.Buffer(), sizeof(Type)*m_nRows*m_nCols);
}

template <class Type> 
CMatrix<Type>::CMatrix(int Len)
{
    m_nRows = 0;
    m_nCols = 0;
    m_pData = NULL;

	SetSize(Len);
}

template <class Type> 
CMatrix<Type>::~CMatrix()
{
	CleanUp();
}

template <class Type>
bool CMatrix<Type>::SetSize(int Rows, int Cols)
{
    if (Rows == 0 && Cols == 0)
    {
        CleanUp();
        return true;
    }

	// check if both Rows and Cols are valid
	if (Rows < 1 || Cols < 1)
	{
        CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix SetSize: Row or Column input argument invalid",
            excep_attrib(Rows<1? "row" : "col") <<
            excep_this_index(Rows<1? Rows : Cols));
#endif
        return false;
	}

	// check if the current buffer size is alright
	if (m_nRows*m_nCols == Rows*Cols)
	{
		m_nRows = Rows;
		m_nCols = Cols;
		return true;
	}

	CleanUp();

	m_pData = new Type[Rows*Cols];
	if (m_pData)
	{
		m_nRows = Rows;
		m_nCols = Cols;
		return true;
	}
	else
	{
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT1("Matrix SetSize: Cannot allocate memory");
#else
		return false;
#endif
	}
}

template <class Type>
bool CMatrix<Type>::SetSize(int Rows, int Cols, Type Val)
{
    // check if both Rows and Cols are valid
    if (Rows < 1 || Cols < 1)
    {
        CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix SetSize: Row or Column input argument invalid",
            excep_attrib(Rows<1? "row" : "col") <<
            excep_this_index(Rows<1? Rows : Cols));
#endif
        return false;
    }
	else if (SetSize(Rows, Cols))
	{
#pragma warning(push)
#pragma warning(disable: 4996)
        std::fill_n(&m_pData[0],m_nRows*m_nCols,Val);
#pragma warning(pop)
		return true;
	}
	else
	{
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::SetSize(int Len)
{
	return SetSize(Len, 1);
}

template <class Type>
void CMatrix<Type>::SetZeros()
{
	if (isValid())
	{
		memset(m_pData, 0, sizeof(Type)*m_nRows*m_nCols);
	}
}

template <class Type>
bool CMatrix<Type>::SubMat(int r, int rr, int c, int cc, CMatrix<Type>& M) const
{
	if (r>=0 && rr<m_nRows && rr>=r && c>=0 && cc<m_nCols && cc>=c &&
		M.SetSize(rr-r+1, cc-c+1))
	{
		for(int i=r; i<=rr; i++)
		{
			for(int j=c; j<=cc; j++)
			{
				M(i-r,j-c) = (*this)(i,j);
			}
		}

		return true;
	}
	else
	{
		M.CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix SubMat: Provided bad indices into matrix",
            excep_attrib(r<0? "r" : rr>=Rows()? "rr" : c<0? "c" : "cc") <<
            excep_max_idx  (r<0 || r>=Rows()? Rows()-1 : Cols()-1) <<
            excep_this_index(r<0? r : rr>=Rows()? rr : c<0? c : cc));
#endif
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::SubMat(int r, int rr, int c, int cc) const
{
	CMatrix<Type> M;

	SubMat(r, rr, c, cc, M);

	return M;
}

template <class Type>
bool CMatrix<Type>::GetRow(int r, CMatrix<Type>& M) const
{
	return SubMat(r,r,0,m_nCols-1,M);
}

template <class Type>
CMatrix<Type> CMatrix<Type>::GetRow(int r) const
{
	return (*this)(r,r,0,m_nCols-1);
}

template <class Type>
bool CMatrix<Type>::GetCol(int c, CMatrix<Type>& M) const
{
	return SubMat(0,m_nRows-1,c,c,M);
}

template <class Type>
CMatrix<Type> CMatrix<Type>::GetCol(int c) const
{
	return (*this)(0,m_nRows-1,c,c);
}

template <class Type>
double CMatrix<Type>::GetVecNorm() const
{
	double norm = 0.0;
	double val;

	for(int i=0; i<m_nRows*m_nCols; i++)
	{
		val = double(m_pData[i]);
		norm += val*val;
	}

	return sqrt(norm);
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Homogeneous(Type lastdim)
{
	CMatrix<Type> result(m_nRows+1);

	if (m_nCols == 1)
	{
		memcpy(result.m_pData, m_pData, sizeof(Type)*m_nRows);
	}
	else
	{
		for (int i = 0; i < m_nRows; i++)
			result(i) = (*this)(i);
	}

	result(m_nRows) = lastdim;

	return result;
}

template <class Type>
bool CMatrix<Type>::isInside(int r, int c) const
{
	return (r>=0 && r<m_nRows && c>=0 && c<m_nCols);
}

template <class Type>
Type CMatrix<Type>::Trace() const
{
	Type sum = (Type) 0;

	int l = __min(m_nRows, m_nCols);

	for(int i=0; i<l; i++)
	{
		sum += (*this)(i,i);
	}

	return sum;
}

template <class Type>
bool CMatrix<Type>::QR(CMatrix<Type>& Q, CMatrix<Type>& R) const
{
	// check if this matrix has been initialized properly
	if (m_nRows==0)
	{
		Q.CleanUp();
		R.CleanUp();
		return false;
	}

	// set the sizes of Q and R
	if (!Q.SetSize(m_nRows, m_nCols) || !R.SetSize(m_nCols, m_nCols))
	{
		Q.CleanUp();
		R.CleanUp();
		return false;
	}

	Q = *this;
	R.SetZeros();

	int i, j, k;

	for(j=0; j<m_nCols; j++)
	{	
		double norm, val, scale;

		// find the largest element in the current column
		scale = 0.0;
		for(i=0; i<m_nRows; i++)
		{
			// modified by jerry, originally: scale = __max(scale,double(Q(i,j)));
			scale = __max(scale,abs(double(Q(i,j))));
		}

		if (scale == 0.0)
		{
			continue;
		}

		// compute the vector norm for the current column
		norm = 0.0;
		for(i=0; i<m_nRows; i++)
		{
			val = Q(i,j)/scale;
			norm += val*val;
		}
		norm = sqrt(norm)*scale;

		// assign the norm to the diagonals of R
		R(j,j) = (Type) norm;

		// make the current column a unit vector
		for(i=0; i<m_nRows; i++)
		{
			Q(i,j) = (Type) ((Q(i,j))/norm);
		}

		for(k=j+1; k<m_nCols; k++)
		{
			// compute the projection onto the current column
			norm = 0.0;
			for(i=0; i<m_nRows; i++)
			{
				norm += double(Q(i,j))*double(Q(i,k));
			}

			// update R
			R(j,k) = (Type) norm;

			// remove the component due to the current column
			for(i=0; i<m_nRows; i++)
			{
				Q(i,k) = Q(i,k) - (Type) (norm*double(Q(i,j)));
			}
		}
	}

	return true;
}

template <class Type>
bool CMatrix<Type>::QR2(CMatrix<Type>& Q, CMatrix<Type>& R) const
{
	// check if this matrix has been initialized properly
	if (m_nRows==0)
	{
		Q.CleanUp();
		R.CleanUp();
		return false;
	}

	// set the sizes of Q and R
	if (!Q.SetSize(m_nRows, m_nCols) || !R.SetSize(m_nCols, m_nCols))
	{
		Q.CleanUp();
		R.CleanUp();
		return false;
	}

	Q = *this;
	R.SetZeros();

	int i, j, k;

	for(j=m_nCols-1; j>=0; j--)
	{	
		double norm, val, scale;

		// find the largest element in the current column
		scale = 0.0;
		for(i=0; i<m_nRows; i++)
		{
			// modified by jerry, originally: scale = __max(scale,double(Q(i,j)));
			scale = __max(scale,abs(double(Q(i,j))));
		}

		if (scale == 0.0)
		{
			continue;
		}

		// compute the vector norm for the current column
		norm = 0.0;
		for(i=0; i<m_nRows; i++)
		{
			val = Q(i,j)/scale;
			norm += val*val;
		}
		norm = sqrt(norm)*scale;

		// assign the norm to the diagonals of R
		R(j,j) = (Type) norm;

		// make the current column a unit vector
		for(i=0; i<m_nRows; i++)
		{
			Q(i,j) = (Type) ((Q(i,j))/norm);
		}

		for(k=j-1; k>=0; k--)
		{
			// compute the projection onto the current column
			norm = 0.0;
			for(i=0; i<m_nRows; i++)
			{
				norm += double(Q(i,j))*double(Q(i,k));
			}

			// update R
			R(j,k) = (Type) norm;

			// remove the component due to the current column
			for(i=0; i<m_nRows; i++)
			{
				Q(i,k) = Q(i,k) - (Type) (norm*double(Q(i,j)));
			}
		}
	}

	return true;
}

template <class Type>
bool CMatrix<Type>::Chol(CMatrix<Type>& L) const
{
	if (m_nCols!=m_nRows || !L.SetSize(m_nCols, m_nRows))
	{
		L.CleanUp();
		return false;
	}

	L.SetZeros();

	double sum;

	for(int i=0; i<m_nRows; i++)
	{
		for(int j=i; j<m_nRows; j++)
		{
			sum = (*this)(i,j);
			for(int k=i-1; k>=0; k--)
			{
				sum -= L(i,k)*L(j,k);
			}

			if (i==j)
			{
				if (sum <= 0.0)
				{
					// not positive definite
					L.CleanUp();
					return false;
				}
				L(i,i) = (Type) sqrt(sum);
			}
			else
			{
				L(j,i) = (Type) sum/L(i,i);
			}
		}
	}

	return true;
}

template <class Type>
bool CMatrix<Type>::SVD(CMatrix<Type>& U, CMatrix<Type>& D, CMatrix<Type>& V) const
{
	int flag, i, its, j, jj, k, l, nm;
	double c, f, h, s, x, y, z;
	double anorm=0.0, g=0.0, scale=0.0;

	int m = __max(m_nRows, m_nCols);
	int n = m_nCols;

	// initialize the result matrices
	if (!U.SetSize(m, n) || !D.SetSize(n) || !V.SetSize(n, n))
	{
		U.CleanUp();
		D.CleanUp();
		V.CleanUp();
		return false;
	}
	U.SetZeros();

	// copy the content of this matrix to the upper left submatrix of U
//	int column_width = sizeof(Type)*m_nCols;
//	for(i=0; i<m_nRows; i++)
//	{
//		memcpy(U.Row(i), (*this).Row(i), column_width);
//	}
	memcpy(U.Buffer(),(*this).Buffer(),sizeof(Type)*m_nRows*m_nCols);

	Type* rv1 = new Type[n];
	if (!rv1)
	{
		U.CleanUp();
		D.CleanUp();
		V.CleanUp();
		return false;
	}

	for(i=0; i<n; i++)
	{
		l = i+1;
		rv1[i] = (Type) (scale*g);
		g = s = scale = 0.0;
		if (i < m)
		{
			for(k=i; k<m; k++)
				scale += fabs((double)U(k,i));
			if (scale) 
			{
				for(k=i; k<m; k++)
				{
					U(k,i) = (Type) (U(k,i)/scale);
					s += double(U(k,i)*U(k,i));
				}
				f = double(U(i,i));
				g = -_copysign(sqrt(s), f);
				h = f*g-s;
				U(i,i) = (Type) (f-g);
				if (i!=n-1) 
				{
					for(j=l; j<n; j++) 
					{
						for(s=0.0, k=i; k<m; k++)
							s += double(U(k,i)*U(k,j));
						f = s/h;
						for(k=i; k<m; k++)
							U(k,j) += (Type) (f*U(k,i));
					}
				}
				for(k=i; k<m; k++)
					U(k,i) = (Type) (U(k,i)*scale);
			}
		}

		D(i) = (Type) (scale*g);
		g = s = scale = 0.0;
		if (i<m && i!=n-1)
		{
			for (k=l; k<n; k++)
				scale += fabs((double)U(i,k));
			if (scale) 
			{
				for(k=l; k<n; k++) 
				{
					U(i,k) =  (Type) (U(i,k)/scale);
					s += (Type) (U(i,k)*U(i,k));
				}
				f = double(U(i,l));
				g = -_copysign(sqrt(s), f);
				h = f*g-s;
				U(i,l) = (Type) (f-g);
				for(k=l ; k<n; k++)
					rv1[k] = (Type) (U(i,k)/h);
				if (i!=m-1) 
				{
					for(j=l; j<m; j++) 
					{
						for(s=0.0, k=l; k<n; k++)
							s += double(U(j,k)*U(i,k));
						for(k=l; k<n; k++)
							U(j,k) = (Type) (U(j,k)+s*rv1[k]);
					}
				}
				for(k=l; k<n; k++)
					U(i,k) = (Type) (U(i,k)*scale);
			}
		}
		anorm = __max(anorm, fabs((double)D(i))+fabs((double)rv1[i]));
	}

	for(i=n-1; i>=0; i--)
	{
		if (i<n-1) 
		{
			if (g) 
			{
				for(j=l; j<n; j++)
					V(j,i) = (Type) ((U(i,j)/U(i,l))/g);
				for (j=l; j<n; j++) 
				{
					for(s=0.0, k=l; k<n; k++) 
						s += double(U(i,k)*V(k,j));
					for(k=l; k<n; k++)
						V(k,j) = (Type) (V(k,j)+s*V(k,i));
				}
			}
			for(j=l; j<n; j++)
				V(i,j) = V(j,i) = (Type) 0.0;
		}
		V(i,i) = (Type) 1.0;
		g = double(rv1[i]);
		l = i;
	}
	for(i=n-1; i>=0; i--)
	{
		l = i+1;
		g = D(i);
		if (i<n-1)
			for(j=l; j<n; j++)
				U(i,j) = (Type) 0.0;
		if (g) 
		{
			g = 1.0/g;
			if (i!=n-1)
			{
				for(j=l; j<n; j++)
				{
					for(s=0.0, k=l; k<m; k++)
						s += double(U(k,i)*U(k,j));
					f = (s/U(i,i))*g;
					for(k=i; k<m; k++)
						U(k,j) = (Type) (U(k,j)+f*U(k,i));
				}
			}
			for(j=i; j<m; j++)
				U(j,i) = (Type) (U(j,i)*g);
		} 
		else 
		{
			for(j=i; j<m; j++) 
				U(j,i) = (Type) 0.0;
		}
		++U(i,i);
	}
	for(k=n-1; k>=0;k--)
	{
		for(its=0; its<30; its++)
		{
			flag = 1;
			for(l=k; l>=0; l--) 
			{
				nm = l-1;
				if (fabs((double)rv1[l])+anorm==anorm)
				{
					flag = 0;
					break;
				}
				if (fabs((double)D(nm))+anorm==anorm)
					break;
			}
			if (flag) 
			{
				c = 0.0;
				s = 1.0;
				for(i=l; i<=k; i++) {
					f = s*rv1[i];
					if (fabs((double)f)+anorm!=anorm) 
					{
						g = double(D(i));
						h = Pythag(f,g);
						D(i) = (Type) h;
						h = 1.0/h;
						c = g*h;
						s = -f*h;
						for(j=0; j<m; j++) 
						{
							y = double(U(j,nm));
							z = double(U(j,i));
							U(j,nm) = (Type) (y*c+z*s);
							U(j,i) = (Type) (z*c-y*s);
						}
					}
				}
			}
			z = D(k);
			if (l==k) 
			{
				if (z<0.0) 
				{
					D(k) = (Type) (-z);
					for(j=0; j<n; j++) 
						V(j,k) = -V(j,k);
				}
				break;
			}

			x = double(D(l));
			nm = k-1;
			y = double(D(nm));
			g = double(rv1[nm]);
			h = double(rv1[k]);
			f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g = Pythag(f, 1.0);
			f = ((x-z)*(x+z)+h*((y/(f+_copysign(g, f)))-h))/x;
			c = s = 1.0;
			for(j=l; j<=nm; j++)
			{
				i = j+1;
				g = double(rv1[i]);
				y = double(D(i));
				h = s*g;
				g = c*g;
				z = Pythag(f, h);
				rv1[j] = (Type) z;
				c = f/z;
				s = h/z;
				f = x*c+g*s;
				g = g*c-x*s;
				h = y*s;
				y = y*c;
				for(jj=0; jj<n; jj++) 
				{
					x = double(V(jj,j));
					z = double(V(jj,i));
					V(jj,j) = (Type) (x*c+z*s);
					V(jj,i) = (Type) (z*c-x*s);
				}
				z=Pythag(f, h);
				D(j) = (Type) z;
				if (z) 
				{
					z = 1.0/z;
					c = f*z;
					s = h*z;
				}
				f = (c*g)+(s*y);
				x = (c*y)-(s*g);
				for(jj=0; jj<m; jj++)
				{
					y = double(U(jj,j));
					z = double(U(jj,i));
					U(jj,j) = (Type) (y*c+z*s);
					U(jj,i) = (Type) (z*c-y*s);
				}
			}
			rv1[l] = (Type) 0.0;
			rv1[k] = (Type) f;
			D(k) = (Type) x;
		}
	}
	delete rv1;

	double wmax = 0;
	for(i=0; i<n; i++)
	{
		if (D(i)>wmax)
			wmax = double(D(i));
	}

	double wmin = wmax*singular_value_cutoff;
	for(i=0; i<n; i++)
	{
		if (D(i)<wmin)
			D(i) = (Type) 0;
	}

	return true;
}

template <class Type>
bool CMatrix<Type>::SVD2(CMatrix<Type>& U, CMatrix<Type>& D, CMatrix<Type>& V) const
{
	// perform singular value decomposition
	if (!SVD(U, D, V))
	{
		return false;
	}

	int l = D.Len();

	// sorting is not necessary if there are less than 2 values
	if (l<2)
		return true;

	for(int c=0; c<l-1; c++)
	{
		Type wmax = D(c);
		int cc = 0;

		for(int i=c+1; i<l; i++)
		{
			if (D(i)>wmax)
			{
				wmax = D(i);
				cc = i;
			}
		}
		
		if (cc)
		{
			U.SwapCols(c, cc);
			D.Swap(c, cc);
			V.SwapCols(c, cc);
		}
	}

	return true;
}

template <class Type>
double CMatrix<Type>::Det() const
{
	CMatrix<Type> U, D, V;
	double det = 1;
	if (!SVD(U, D, V))
		return 0;

	int l = D.Len();
	for(int c=0; c<l; c++)
		det*=D(c);

	return det;
}

template <class Type>
bool CMatrix<Type>::Transpose(CMatrix<Type>& M) const
{
	if (M.SetSize(m_nCols, m_nRows))
	{
		for(int r=0; r<m_nRows; r++)
		{
			for(int c=0; c<m_nCols; c++)
			{
				M(c,r) = (*this)(r,c);
			}
		}
		return true;
	}
	else
	{
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Transpose() const
{
	CMatrix<Type> result;

	(*this).Transpose(result);

	return result;
}

template <class Type>
bool CMatrix<Type>::Inverse(CMatrix<Type>& M) const
{
	CMatrix<Type> U, D, V, W;

	if (!SVD(U, D, V))
	{
		M.CleanUp();
		return false;
	}

	int l = D.Len();

	if (!W.SetSize(l, l))
	{
		M.CleanUp();
		return false;
	}

	W.SetZeros();

	for(int i=0; i<l; i++)
	{
		Type d = D(i);
		if (d>singular_value_cutoff)
		{
			W(i,i) = (Type) (1/d);
		}
	}

	M = V*W*(U.Transpose());

	if (M.isValid())
	{
		return true;
	}
	else
	{
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Inverse() const
{
	CMatrix<Type> U, D, V, W;

	if (!SVD(U, D, V))
	{
		return U;
	}

	int l = D.Len();

	if (!W.SetSize(l, l))
	{
		return U;
	}

	W.SetZeros();

	for(int i=0; i<l; i++)
	{
		Type d = D(i);
		if (d>singular_value_cutoff)
		{
			W(i,i) = (Type) (1/d);
		}
	}

	return V*W*(U.Transpose());
}

template <class Type>
bool CMatrix<Type>::Solve(const CMatrix<Type>& B, CMatrix<Type>& X) const
{
	if (B.Rows()!=m_nRows)
	{
		X.CleanUp();
		return false;
	}
	else
	{
		X = Inverse()*B;

		if (X.isValid())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Solve(const CMatrix<Type>& B) const
{
	CMatrix<Type> result;

	if (B.Rows()!=m_nRows)
		return result;
	else
		return (Inverse()*B);
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::Diag(const CMatrix<Type>& M)
{
	// to avoid break down in situations like A.Diag(A)
	CMatrix<Type> tmp = M;

	if (tmp.isValid())
	{
		int l = tmp.Len();

		if (SetSize(l, l))
		{
			SetZeros();
			for(int i=0; i<l; i++)
			{
				(*this)(i,i) = tmp(i);
			}
		}
	}
	else
	{
		CleanUp();
	}

	return *this;
}
	
template <class Type>
CMatrix<Type>& CMatrix<Type>::Diag(const Type M[], int l)
{
	// to avoid break down in situations like A.Diag(A.Buffer())
	Type* tmp = new Type[l];

	if (tmp!=NULL)
	{
		for(int i=0; i<l; i++)
		{
			tmp[i] = M[i];
		}

		if (SetSize(l, l))
		{
			SetZeros();
			for(int i=0; i<l; i++)
			{
				(*this)(i,i) = tmp[i];
			}
		}

		delete tmp;
	}
	else
	{
		CleanUp();
	}

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::Diag(Type V)
{
	int l = __min(m_nRows, m_nCols);

	SetZeros();

	for(int i=0; i<l; i++)
	{
		(*this)(i,i) = V;
	}

	return *this;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Diag() const
{
	CMatrix<Type> result;

	int l = __min(m_nRows, m_nCols);

	if (result.SetSize(l))
	{
		for(int i=0; i<l; i++)
		{
			result(i) = (*this)(i,i);
		}
	}

	return result;
}

template <class Type>
bool CMatrix<Type>::Set(int r, int c, const CMatrix<Type>& M)
{
	int rr = r+M.Rows()-1;
	int cc = c+M.Cols()-1;

	if (r>=0 && rr>=r && rr<m_nRows &&
		c>=0 && cc>=c && cc<m_nCols)
	{
		for(int i=r; i<=rr; i++)
		{
			for(int j=c; j<=cc; j++)
			{
				(*this)(i,j) = M(i-r,j-c);
			}
		}

		return true;
	}
	else
	{
#ifdef MATRIX_THROW_EXCEPTIONS
        if (rr>=Rows() || cc>=Cols())
            MATRIX_THROW_EXCEPT2("Matrix Set: Matrix M is too big",
                excep_attrib(rr>=Rows()? "Rows" : "Cols") <<
                excep_size_this(Rows(),Cols()) <<
                excep_size_other(M.Rows(),M.Cols()));
        else if (rr<r || cc<c)
            MATRIX_THROW_EXCEPT1("Matrix Set: Trying to Set() using empty matrix M");
        else
            MATRIX_THROW_EXCEPT2("Matrix Set: Provided bad indices into matrix",
                excep_attrib(r<0? "Rows" : "Cols") <<
                excep_this_index(r<0? r : c) <<
                excep_size_this(Rows(),Cols()));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::Set(int r, int c, const CMatrix<Type>& M, int r1, int r2, int c1, int c2)
{
	int rr = r+r2-r1;
	int cc = c+c2-c1;

	if (r1>=0 && r2>=r1 && r2<M.Rows() &&
		c1>=0 && c2>=c1 && c2<M.Cols() &&
		r >=0 && rr>=r  && rr<m_nRows  &&
		c >=0 && cc>=c  && cc<m_nCols)
	{
		for(int i=r1; i<=r2; i++)
		{
			for(int j=c1; j<=c2; j++)
			{
				(*this)(r+i-r1,c+j-c1) = M(i,j);
			}
		}

		return true;
	}
	else
	{
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix Set: Provided bad indices into matrix",
            excep_size_this (Rows(),Cols()) <<
            excep_size_other(M.Rows(),M.Cols()));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::Set(int r, int rr, int c, int cc, const Type& T)
{
	if (r>=0 && rr>=r && rr<m_nRows &&
		c>=0 && cc>=c && cc<m_nCols)
	{
		for(int i=r; i<=rr; i++)
		{
			for(int j=c; j<=cc; j++)
			{
				(*this)(i,j) = T;
			}
		}

		return true;
	}
	else
    {
#ifdef MATRIX_THROW_EXCEPTIONS
        if (rr>=Rows() || cc>=Cols())
            MATRIX_THROW_EXCEPT2("Matrix Set: Index out of range",
                excep_attrib(rr>=Rows()? "Rows" : "Cols") <<
                excep_size_this (Rows(),Cols()) <<
                excep_max_idx  (rr>=Rows()? Rows()-1: Cols()-1) <<
                excep_this_index(rr>=Rows()? rr : cc));
        else if (rr<r || cc<c)
            MATRIX_THROW_EXCEPT2("Matrix Set: Trying to Set() using empty range",
                excep_attrib(rr<r? "Rows" : "Cols"));
        else
            MATRIX_THROW_EXCEPT2("Matrix Set: Provided bad indices into matrix",
                excep_attrib(r<0? "Rows" : "Cols") <<
                excep_size_this (Rows(),Cols()) <<
                excep_this_index(r<0? r : c));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::SetRow(int r, const CMatrix<Type>& Row)
{
	if (r>=0 && r<m_nRows && Row.Rows()==1 && Row.Cols()==m_nCols)
	{
		for(int c=0; c<m_nCols; c++)
		{
			(*this)(r,c) = Row(c);
		}

		return true;
	}
	else
	{
#ifdef MATRIX_THROW_EXCEPTIONS
        if (r<0 && r>=Rows())
            MATRIX_THROW_EXCEPT2("Matrix SetRow: Index out of range",
                excep_size_this (Rows(),Cols()) <<
                excep_max_idx  (Rows()-1) <<
                excep_this_index(r));
        else if (Row.Rows()!=1)
            MATRIX_THROW_EXCEPT2("Matrix SetRow: Insertee not a single row",
                excep_size_other (Row.Rows(),Row.Cols()));
        else
            MATRIX_THROW_EXCEPT2("Matrix SetRow: Insertee's number of columns does not match that of matrix inserted to",
                excep_size_this (Rows(),Cols()) <<
                excep_size_other (Row.Rows(),Row.Cols()));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::SetCol(int c, const CMatrix<Type>& Col)
{
	if (c>=0 && c<m_nCols && Col.Cols()==1 && Col.Rows()==m_nRows)
	{
		for(int r=0; r<m_nRows; r++)
		{
			(*this)(r,c) = Col(r);
		}

		return true;
	}
	else
    {
#ifdef MATRIX_THROW_EXCEPTIONS
        if (c<0 && c>=Cols())
            MATRIX_THROW_EXCEPT2("Matrix SetCol: Index out of range",
                excep_size_this (Rows(),Cols()) <<
                excep_max_idx  (Cols()-1) <<
                excep_this_index(c));
        else if (Col.Cols()!=1)
            MATRIX_THROW_EXCEPT2("Matrix SetCol: Insertee not a single column",
                excep_size_other (Col.Rows(),Col.Cols()));
        else
            MATRIX_THROW_EXCEPT2("Matrix SetCol: Insertee's number of rows does not match that of matrix inserted to",
                excep_size_this (Rows(),Cols()) <<
                excep_size_other (Col.Rows(),Col.Cols()));
#endif
        return false;
	}
}

template <class Type>
bool CMatrix<Type>::SwapRows(int r, int rr)
{
	if (r==rr)
	{
		return true;
	}
	else if (r>=0 && r<m_nRows && rr>=0 && rr<m_nRows)
	{
		Type temp;
		for(int c=0; c<m_nCols; c++)
		{
			temp = (*this)(r,c);
			(*this)(r,c) = (*this)(rr,c);
			(*this)(rr,c) = temp;
		}

		return true;
	}
	else
	{
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix SwapRows: Provided bad indices into matrix",
            excep_attrib(r>=0 || r<m_nRows? "r" : "rr") <<
            excep_max_idx(m_nRows-1) <<
            excep_this_index(r>=0 || r<m_nRows? r : rr));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::SwapCols(int c, int cc)
{
	if (c==cc)
	{
		return true;
	}
	else if (c>=0 && c<m_nCols && cc>=0 && cc<m_nCols)
	{
		Type temp;
		for(int r=0; r<m_nRows; r++)
		{
			temp = (*this)(r,c);
			(*this)(r,c) = (*this)(r,cc);
			(*this)(r,cc) = temp;
		}

		return true;
	}
	else
    {
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix SwapCols: Provided bad indices into matrix",
            excep_attrib(c>=0 || c<m_nCols? "c" : "cc") <<
            excep_max_idx(m_nCols-1) <<
            excep_this_index(c>=0 || c<m_nCols? c : cc));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::Swap(int i, int j)
{
	if (i==j)
	{
		return true;
	}
	else if (i>=0 && i<Len() && j>=0 && j<Len())
	{
		Type temp;
		
		temp = m_pData[i];
		m_pData[i] = m_pData[j];
		m_pData[j] = temp;

		return true;
	}
	else
    {
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix Swap: Provided bad indices into matrix",
            excep_attrib(i>=0 || i<Len()? "i" : "j") <<
            excep_max_idx(Len()-1) <<
            excep_this_index(i>=0 || i<Len()? i : j));
#endif
		return false;
	}
}

template <class Type>
bool CMatrix<Type>::RepMat(int r, int c, CMatrix<Type>& M) const
{
	if (M.SetSize(r*m_nRows, c*m_nCols))
	{
		for(int i=0; i<r; i++)
		{
			for(int j=0; j<c; j++)
			{
				M.Set(i*m_nRows, j*m_nCols, *this);
			}
		}

		return true;
	}
	else
	{
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::RepMat(int r, int c) const
{
	CMatrix<Type> result;

	RepMat(r,c,result);

	return result;
}

template <class Type>
bool CMatrix<Type>::Tx(CMatrix<Type>& M) const
{
	if (Len()==3 && M.SetSize(3,3))
	{
		// Tx = [ 0    -T(3)  T(2)
		//        T(3)  0    -T(1)
		//       -T(2)  T(1)  0   ];

		M(0,0) =      0     ; M(0,1) = -(*this)(2); M(0,2) =  (*this)(1);
		M(1,0) =  (*this)(2); M(1,1) =      0     ; M(1,2) = -(*this)(0);
		M(2,0) = -(*this)(1); M(2,1) =  (*this)(0); M(2,2) =      0     ;

		return true;
	}
	else
	{
		M.CleanUp();
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Tx() const
{
	CMatrix<Type> result;

	if (Len()>=3 && result.SetSize(3,3))
	{
		// Tx = [ 0    -T(3)  T(2)
		//        T(3)  0    -T(1)
		//       -T(2)  T(1)  0   ];

		result(0,0) =      0     ; result(0,1) = -(*this)(2); result(0,2) =  (*this)(1);
		result(1,0) =  (*this)(2); result(1,1) =      0     ; result(1,2) = -(*this)(0);
		result(2,0) = -(*this)(1); result(2,1) =  (*this)(0); result(2,2) =      0     ;
	}

	return result;
}

template <class Type>
bool CMatrix<Type>::Conv(const CMatrix<Type>& M, CMatrix<Type>& Result) const
{
	int len1 = Len();
	int len2 = M.Len();
	int len  = len1+len2-1;

	if (len1<1 || len2<1 || !Result.SetSize(len))
	{
		Result.CleanUp();
		return false;
	}

	Result.SetZeros();

	for(int i=0; i<len1; i++)
	{
		for(int j=0; j<len2; j++)
		{
			Result(i+j) += (*this)(i)*M(j);
		}
	}

	return true;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Conv(const CMatrix<Type>& M) const
{
	CMatrix<Type> result;

	Conv(M,result);

	return result;
}
//shuda
template <class Type>
void CMatrix<Type>::Normalize()
{
	double dNorm = GetVecNorm();
	if(dNorm<1e-20) return;
	(*this)/=(Type)dNorm;
}
//shuda
template <class Type>
Type CMatrix<Type>::NCC( const CMatrix<Type>& M_ ) const
{
	double dM1 = Mean();
	double dM2 = M_.Mean();
	CMatrix<Type> M1=(*this)-(Type)dM1;
	M1.Normalize();
	CMatrix<Type> M2= M_-(Type)dM2;
	M2.Normalize();
	return dot(M1,M2);
	
}

template <class Type>
Type CMatrix<Type>::NCC2( const CMatrix<Type>& M_ ) const
{
	CMatrix<Type> M1=(*this);
	M1.Normalize();
	CMatrix<Type> M2= M_;
	M2.Normalize();
	return dot(M1,M2);
}
template <class Type>
bool CMatrix<Type>::Times(const CMatrix<Type>& M, CMatrix<Type>& Result) const
{
	if (m_nRows==M.Rows() && m_nCols==M.Cols() && Result.SetSize(m_nRows,m_nCols))
	{
		int l = Len();
		for(int i=0; i<l ; i++)
		{
			Result(i) = (Type) (*this)(i)*M(i);
		}
		return true;
	}
	else
	{
		Result.CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix Times: Inputs do not have same shape",
            excep_attrib(Rows()!=M.Rows()? "row" : "col") <<
            excep_max_idx(Rows()!=M.Rows()? Rows() : Cols()) <<
            excep_this_index(Rows()!=M.Rows()? M.Rows() : M.Cols()) <<
            excep_size_this(Rows(),Cols()) <<
            excep_size_other(M.Rows(),M.Cols()));
#endif
		return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Times(const CMatrix<Type>& M) const
{
	CMatrix<Type> result;

    Times(M,result);

	return result;
}

template <class Type>
bool CMatrix<Type>::Div(const CMatrix<Type>& M, CMatrix<Type>& Result) const
{
	if (m_nRows==M.Rows() && m_nCols==M.Cols() && Result.SetSize(m_nRows,m_nCols))
	{
		int l = Len();
		for(int i=0; i<l ; i++)
		{
			Result(i) = (Type) (*this)(i)/M(i);
		}
		return true;
	}
	else
	{
		Result.CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix Div: Inputs do not have same shape",
            excep_attrib(Rows()!=M.Rows()? "row" : "col") <<
            excep_max_idx(Rows()!=M.Rows()? Rows() : Cols()) <<
            excep_this_index(Rows()!=M.Rows()? M.Rows() : M.Cols()) <<
            excep_size_this(Rows(),Cols()) <<
            excep_size_other(M.Rows(),M.Cols()));
#endif
        return false;
	}
}

template <class Type>
CMatrix<Type> CMatrix<Type>::Div(const CMatrix<Type>& M) const
{
	CMatrix<Type> result;

	Div(M,result);

	return result;
}


template <class Type>
bool CMatrix<Type>::RowMean(CMatrix<Type>& M) const
{
	// allocate memory
	if (!M.SetSize(m_nRows,1))
	{
		return false;
	}

	M.SetZeros();

	for(int r=0; r<m_nRows; r++)
	{
		double val = 0.0;
		for(int c=0; c<m_nCols; c++)
		{
			val += double((*this)(r,c));
		}

		M(r) = (Type) (val/double(m_nCols));
	}

	return true;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::RowMean() const
{
	CMatrix<Type> M;

	RowMean(M);

	return M;
}

template <class Type>
bool CMatrix<Type>::ColMean(CMatrix<Type>& M) const
{
	// allocate memory
	if (!M.SetSize(1,m_nCols))
	{
		return false;
	}

	M.SetZeros();

	for(int c=0; c<m_nCols; c++)
	{
		double val = 0.0;
		for(int r=0; r<m_nRows; r++)
		{
			val += double((*this)(r,c));
		}

		M(c) = (Type) (val/double(m_nRows));
	}

	return true;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::ColMean() const
{
	CMatrix<Type> M;

	ColMean(M);

	return M;
}

template <class Type>
double CMatrix<Type>::Mean() const
{
	double val = 0.0;
	int len = m_nRows*m_nCols;

	for(int i=0; i<len; i++)
	{
		val += double(m_pData[i]);
	}

	return (val/double(len));
}
template <class Type>
double CMatrix<Type>::Min() const
{
	double dMin=10000000;
	for(int i=0;i<Len();i++){
		if(dMin>(*this)(i))
			dMin=(*this)(i);
	}
	return dMin;
}
template <class Type>
double CMatrix<Type>::Max() const
{
	double dMax=-10000000;
	for(int i=0;i<Len();i++){
		if(dMax<(*this)(i))
			dMax=(*this)(i);
	}
	return dMax;
}
template <class Type>
double CMatrix<Type>::Max(int *pc,int *pr) const
{
	double dMax=-10000000;
	for(int c=0;c<Cols();c++)for(int r=0;r<Rows();r++)
	{
		if(dMax<(*this)(c,r)){	
			dMax=(*this)(c,r);
			*pc = c; *pr = r;
		}
	}
	return dMax;
}
template <class Type>
void CMatrix<Type>::CleanUp()
{
	m_nRows = 0;
	m_nCols = 0;

	if (m_pData)
	{
		delete [] m_pData;
	}
	m_pData = NULL;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator () (int r, int rr, int c, int cc) const
{
	CMatrix<Type> result;
	if (r>=0 && rr>=r && rr<m_nRows &&
		c>=0 && cc>=c && cc<m_nCols &&
		result.SetSize(rr-r+1, cc-c+1))
	{
		for(int i=r; i<=rr; i++)
		{
			for(int j=c; j<=cc; j++)
			{
				result(i-r,j-c) = (*this)(i,j);
			}
		}
    }
#ifdef MATRIX_THROW_EXCEPTIONS
    else
        MATRIX_THROW_EXCEPT1("Matrix M(r,rr,c,cc) operator: Provided bad indices into matrix");
#endif

	return result;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator - () const
{
	CMatrix<Type> result;
	if (result.SetSize(m_nRows, m_nCols))
	{
		for(int i=0; i<m_nRows; i++)
		{
			for(int j=0; j<m_nCols; j++)
			{
				result(i,j) = -(*this)(i,j);
			}
		}
	}

	return result;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator + (const CMatrix<Type>& M) const
{
	CMatrix<Type> result(*this);

	return result+=M;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator - (const CMatrix<Type>& M) const
{
	CMatrix<Type> result(*this);

	return result-=M;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator * (const CMatrix<Type>& M) const
{
	CMatrix<Type> result;
    if (m_nCols==M.Rows() && result.SetSize(m_nRows, M.Cols()))
    {
        result.SetZeros();
        for(int i=0; i<m_nRows; i++)
        {
            for(int j=0; j<M.Cols(); j++)
            {
                for(int k=0; k<m_nCols; k++)
                {
                    result(i,j) += (*this)(i,k) * M(k, j);
                }
            }
        }
    }
    else
    {
        result.CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix *: Inputs do not have compatible shape ((*this).Cols()!=M.Rows())",
        excep_size_this(Rows(),Cols()) <<
        excep_size_other(M.Rows(),M.Cols()));
#endif
    }

	return result;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator + (const Type& T) const
{
	CMatrix<Type> result(*this);

	return result+=T;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator - (const Type& T) const
{
    CMatrix<Type> result(*this);

    return result-=T;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator * (const Type& T) const
{
    CMatrix<Type> result(*this);

    return result*=T;
}

template <class Type>
CMatrix<Type> CMatrix<Type>::operator / (const Type& T) const
{
    CMatrix<Type> result(*this);

    return result/=T;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator = (const CMatrix<Type>& M)
{
	if (this!=&M && SetSize(M.Rows(), M.Cols()))
	{
		memcpy(m_pData, M.Buffer(), sizeof(Type)*m_nRows*m_nCols);
	}

	return *this;
}


template <class Type>
CMatrix<Type>& CMatrix<Type>::operator += (const CMatrix<Type>& M)
{
    if (m_nRows==M.Rows() && m_nCols==M.Cols())
    {
        for(int i=0; i<m_nRows; i++)
        {
            for(int j=0; j<m_nCols; j++)
            {
                (*this)(i,j) += M(i,j);
            }
        }
    }
    else
    {
        CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix plus: Inputs do not have same shape",
            excep_attrib(Rows()!=M.Rows()? "row" : "col") <<
            excep_size_this(Rows(),Cols()) <<
            excep_size_other(M.Rows(),M.Cols()));
#endif
    }

    return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator -= (const CMatrix<Type>& M)
{
    if (m_nRows==M.Rows() && m_nCols==M.Cols())
    {
        for(int i=0; i<m_nRows; i++)
        {
            for(int j=0; j<m_nCols; j++)
            {
                (*this)(i,j) -= M(i,j);
            }
        }
    }
    else
    {
        CleanUp();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix minus: Inputs do not have same shape",
        excep_attrib(Rows()!=M.Rows()? "row" : "col") <<
        excep_size_this(Rows(),Cols()) <<
        excep_size_other(M.Rows(),M.Cols()));
#endif
    }

    return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator *= (const CMatrix<Type>& M)
{
    // need to go through a temporary for matrix multiplication
	*this = (*this)*M;

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator += (const Type& T)
{
	for(int i=0; i<m_nRows; i++)
	{
		for(int j=0; j<m_nCols; j++)
		{
			(*this)(i,j) += T;
		}
	}

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator -= (const Type& T)
{
	for(int i=0; i<m_nRows; i++)
	{
		for(int j=0; j<m_nCols; j++)
		{
			(*this)(i,j) -= T;
		}
	}

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator *= (const Type& T)
{
	for(int i=0; i<m_nRows; i++)
	{
		for(int j=0; j<m_nCols; j++)
		{
			(*this)(i,j) *= T;
		}
	}

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator /= (const Type& T)
{
	for(int i=0; i<m_nRows; i++)
	{
		for(int j=0; j<m_nCols; j++)
		{
			(*this)(i,j) /= T;
		}
	}

	return *this;
}

template <class Type>
CMatrix<Type>& CMatrix<Type>::operator = (const std::vector<Type>& V)
{
    if (!V.empty() && SetSize(V.size(),1))
        memcpy(m_pData, &V[0], sizeof(Type)*m_nRows*m_nCols);

    return *this;
}

//template <class Type>
//void CMatrix<Type>::SerializeTxt(CArchive& ar)
//{
//	CString szBuffer;
//	int i, j;
//	double val;
//
//	if (ar.IsStoring())
//	{
//		for(i=0; i<m_nRows; i++)
//		{
//			for(j=0; j<m_nCols; j++)
//			{
//				val = double((*this)(i,j));
//				szBuffer.Format("%10.6le ", val);
////				int v = int((*this)(i,j));
////				szBuffer.Format("%d", v);
//				ar.WriteString(szBuffer);
//			}
//			ar.WriteString("\n");
//		}
//		ar.WriteString("\n");
//	}
//	else
//	{
//		for(i=0; i<m_nRows; i++)
//		{
//			if (!ar.ReadString(szBuffer))
//				return;
//
//			for(j=0; j<m_nCols; j++)
//			{
//				szBuffer.TrimLeft();
//
//				if (sscanf_s(szBuffer, "%le", &val)!=1)
//					return;
//
//				(*this)(i,j) = (Type) val;
//
//				int idx = szBuffer.FindOneOf(" \t\n");
//				if (idx!=-1)
//				{
//					szBuffer = szBuffer.Mid(idx);
//				}
//			}
//		}
//	}
//}
//
//template<class Type>
//void CMatrix<Type>::Serialize(CArchive &ar)
//{
//	CObject::Serialize(ar);
//
//	if (ar.IsStoring())
//	{
//		ar << m_nRows << m_nCols;
//		for(int i=0; i<m_nRows; i++)
//		{
//			for(int j=0; j<m_nCols; j++)
//			{
//				ar << (*this)(i,j);
//			}
//		}
//	}
//	else
//	{
//		int Rows, Cols;
//		ar >> Rows >> Cols;
//		if (SetSize(Rows, Cols))
//		{
//			for(int i=0; i<m_nRows; i++)
//			{	
//				for(int j=0; j<m_nCols; j++)
//				{
//					ar >> (*this)(i,j);
//				}
//			}
//		}
//	}
//}
//
//template <class Type>
//void CMatrix<Type>::DumpMatrix()
//{
//	CString szBuffer("{");
//	for(int i=0; i<m_nRows; i++)
//	{
//		szBuffer += "{";
//		for(int j=0; j<m_nCols; j++)
//		{
//			double val = double((*this)(i,j));
//			CString tbuf;
//			tbuf.Format("%7.4lf", val);
//			szBuffer += tbuf;
//			if (j<m_nCols-1)
//				szBuffer += ",\t";
//		}
//		if (i < m_nRows-1)
//			szBuffer += "},\n";
//		else
//			szBuffer += "}\n";
//	}
//	szBuffer += "}\n";
//
//	TRACE0(szBuffer.GetBuffer());
//}

template <class Type>
double CMatrix<Type>::Pythag(double a, double b) const
{
	double at,bt;

	at = fabs(a);
	bt = fabs(b);
	
	if (at>bt)
		return (at*sqrt(1.0+(bt/at)*(bt/at)));
	else if (bt==0.0)
		return 0.0;
	else
		return (bt*sqrt(1.0+(at/bt)*(at/bt)));
}

//template <class Type>
//CMatrix<Type>::operator Type() const
//{
//    if (Rows()==1 && Cols()==1)
//    {
//        return (*this)(0);
//    }
//    else
//    {
//        return Type(0);
//#ifdef MATRIX_THROW_EXCEPTIONS
//        MATRIX_THROW_EXCEPT2("Matrix cast to Type scalar: This matrix not 1 row x 1 column ((*this).Cols()!=1 || (*this).Rows()!=1)",
//            excep_size_this(Rows(),Cols()));
//#endif
//    }
//}

template <class Type>
CMatrix<Type>::operator std::vector<Type>() const
{
    if (Rows()==1 || Cols()==1)
    {
        return std::vector<Type>(&(*this)(0),(&(*this)(0))+Len());
    }
    else
    {
        return std::vector<Type>();
#ifdef MATRIX_THROW_EXCEPTIONS
        MATRIX_THROW_EXCEPT2("Matrix cast to Type vector: This matrix not a vector ((*this).Cols()==1 || (*this).Rows()==1)",
            excep_size_this(Rows(),Cols()));
#endif
    }
}

template <class Type>
CMatrix<Type> operator + (const Type& T, const CMatrix<Type>& M)
{
	CMatrix<Type> result(M);

	return result+=T;
}

template <class Type>
CMatrix<Type> operator - (const Type& T, const CMatrix<Type>& M)
{
    CMatrix<Type> result(M);

    return result-=T;
}

template <class Type>
CMatrix<Type> operator * (const Type& T, const CMatrix<Type>& M)
{
    CMatrix<Type> result(M);

    return result*=T;
}

template <class Type>
CMatrix<Type> operator * (const CMatrix<Type>& M, const Type& T)
{
    CMatrix<Type> result(M);

    return result*=T;
}

template <class Type>
CMatrix<Type> operator / (const Type& T, const CMatrix<Type>& M)
{
    CMatrix<Type> result(M);

    return result/=T;
}

template <class Type>
Type dot(const CMatrix<Type>& A, const CMatrix<Type>& B)
{
	if (A.Len()==3 && B.Len()==3)
	{
		return (A(0)*B(0)+A(1)*B(1)+A(2)*B(2));
	}
	else if( A.Len()== B.Len())
	{
		Type dVal=0;
		for(int i=0;i<A.Len();i++){
			dVal += A(i)*B(i);
		}
		return dVal;
	}
	else return ((Type) 0); 
}

template <class Type>
CMatrix<Type> cross(const CMatrix<Type>& A, const CMatrix<Type>& B)
{
	CMatrix<Type> result;

	if (A.Len()==3 && B.Len()==3 && result.SetSize(3))
	{
		result(0) = A(1)*B(2)-A(2)*B(1);
		result(1) = A(2)*B(0)-A(0)*B(2);
		result(2) = A(0)*B(1)-A(1)*B(0);
    }
#ifdef MATRIX_THROW_EXCEPTIONS
    else
    {
        MATRIX_THROW_EXCEPT2("cross: First or second input not length 3",
            excep_attrib    (A.Len()!=3 ? "A" : "B") <<
            excep_size_this (A.Rows(),A.Cols()));
    }
#endif

	return result;
}

template <class Type>
void CMatrix<Type>::GetRankNApproximation( const int& nRank_, CMatrix<Type>* pSingularValues_,CMatrix<Type>* pRank3Approximation_ ) const
{

	CMatrix<Type> U,D,V;
	(*this).SVD2(U,D,V);
	*pSingularValues_=D;
	CMatrix<Type> D__; D__.SetSize(D.Len(),D.Len(),0);
	for(int i=0;i<D.Rows();i++)	{D__(i,i)=D(i);	if(i>=nRank_) break;}
	*pRank3Approximation_=U*D__*(V.Transpose());
}
template <class Type>
double CMatrix<Type>::GetFrobeniusNorm( CMatrix<Type>& M_ ) const
{
	assert(this->Len()==M_.Len());
	double dE=0;double d;
	for(int r=0;r<this->Len();r++)
	{
		d = (*this)(r)-M_(r);
		dE += d*d;
	}
	//*pFNorm_=dE;
	return sqrt(dE/this->Len()); //root mean square.
}

// instantiate the various matrix classes
//Instantiate_Matrix(BYTE);
Instantiate_Matrix(int);
Instantiate_Matrix(float);
Instantiate_Matrix(double);


#undef singular_value_cutoff